// Include files.
#include "EntechFpk.hpp"
#include "TestRobot.hpp"
#include "TestAutonomousController.hpp"

TestAutonomousController::TestAutonomousController(TestRobot &robot) :
	StateMachineController(false),
	m_konamiCode(),
	m_up1
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		0, 2, 0
	),
	m_up2
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		0, 2, 0
	),
	m_down1
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		0, -2, 0
	),
	m_down2
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		0, -2, 0
	),
	m_left1
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		-2, 0, 0
	),
	m_right1
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		2, 0, 0
	),
	m_left2
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		-2, 0, 0
	),
	m_right2
	(
		(MecanumDriveSubsystem&)(robot.m_drive), 
		MecanumDriveSubsystem::DriveModeCartesian, 
		2, 0, 0
	)
{
}

void TestAutonomousController::Initialize()
{
	// Setup Konami Code dance.
	m_konamiCode.AppendState(m_up1);
	m_konamiCode.AppendState(m_up2);
	m_konamiCode.AppendState(m_down1);
	m_konamiCode.AppendState(m_down2);
	m_konamiCode.AppendState(m_left1);
	m_konamiCode.AppendState(m_right1);
	m_konamiCode.AppendState(m_left2);
	m_konamiCode.AppendState(m_right2);
	m_konamiCode.AddTransition(GetFinalState());
	
	// Set dance as the initial state.
	SetInitialState(m_konamiCode);
}
